
#ifndef TCP_SERVER_H
#define TCP_SERVER_H


#include<stdio.h>
#include<stdlib.h>
#include<string.h>
#include<sys/socket.h>
#include<netinet/in.h>
#include<arpa/inet.h>
#include<unistd.h>
#include<iostream>
#include<command_to_drone.h>

using namespace std;

uint8_t map_last[10]={0x00 , 0x02 , 0x04 , 0x06 , 0x07 , 0x08, 0x0a , 0x0c , 0x0e , 0x0f };

uint8_t radioMsg[32]={0x0a, 0xff, 0x05, 0xdc, 0x05, 0xdc, 0x03, 0xea, 0x05, 0xdc, 0x06, 0xdc, 0x06, 0xdc, 0x05, 0xdc, 0x05, 0xdc, 0x05, 0xdc, 0x05, 0xdc, 0x05, 0xdc, 0x05, 0xdc, 0x05, 0xdc, 0x05, 0xdc, 0x05, 0xdc,};
uint8_t temp_variable[32]={0x0a, 0xff, 0x05, 0xdc, 0x05, 0xdc, 0x03, 0xea, 0x05, 0xdc, 0x06, 0xdc, 0x06, 0xdc, 0x05, 0xdc, 0x05, 0xdc, 0x05, 0xdc, 0x05, 0xdc, 0x05, 0xdc, 0x05, 0xdc, 0x05, 0xdc, 0x05, 0xdc, 0x05, 0xdc,};

uint8_t buffer[32];

uint8_t* bf_sbus_trans(uint8_t* source_msg, uint8_t* temp_variable );

uint8_t* bf_sbus_trans(uint8_t* source_msg, uint8_t* temp_variable)
{
    uint8_t* target_msg = temp_variable;
    target_msg[0] = 0x0a;
    target_msg[1] = 0xff;
    uint16_t outl,outh,channels,output;
    for(int i=1;i<8;i++)
    {
        channels=(uint16_t)(source_msg[2*i]<<8)|source_msg[2*i+1];
        outh=(channels-1000)/160;
        outl=(channels-1000-160*outh)/10*16;
        outl+=map_last[channels%10];
        output =(outh<<8)|outl+0xc0;
        target_msg[2*i+1]=(uint8_t)(output&0x00ff);
        target_msg[2*i] = (uint8_t)( output>>8);
    }
    return target_msg;
}

#endif //TCP_SERVER_H